vcMotionTarget

"""Calculate inverse kinematics for given target matrix"""

import vcCore as vc
import vcBehaviors as vc_beh
import vcRobotics as vc_robo

app = vc.getApplication()
comp = vc.getComponent()


async def OnRun():
  
  ctr = comp.findBehavior(vc_beh.vcBehaviorType.ROBOT_CONTROLLER)
  base_name = ctr.Bases[0].Name
  tool_name = ctr.Tools[0].Name
  config = 0
  init_joints = [x.CurrentValue for x in ctr.Joints]
  target_matrix = vc.vcMatrix.new()
  target_matrix.P = vc.vcVector.new(800.0, 0.0, 500.0)
  target_matrix.WPR = vc.vcVector.new(0.0, 180.0, 0.0)
  
  # Solve joint values for given target
  joint_values = calc_inverse(ctr, target_matrix, base_name, tool_name, config, init_joints)
  print('Joint values: ', joint_values)
  
  # Move robot to solved joint values
  for i, j in enumerate(ctr.Joints):
    j.CurrentValue = joint_values[i]
  app.render()


def calc_inverse(controller, target_matrix, base_name, tool_name, config, init_joints):
  """Caclulate inverse kinematics using motion target"""
  
  # Create target
  motion_target = controller.createTarget()
  
  # Set base and tool
  motion_target.BaseName = base_name
  motion_target.ToolName = tool_name
  
  # Set initital joints, used to define joint turns and external axes
  motion_target.JointValues = init_joints
  
  # Set robot config
  motion_target.RobotConfig = config
  
  # Set target matrix
  motion_target.Target = target_matrix
  
  # Return joint values
  return motion_target.JointValues
  

from vcCore import *
from vcFeatures import *
from vcGeometry import *
from vcBehaviors import *
from vcRobotics2 import *

def robot_pose_to_target(controller: vcMotionController, type: vcMotionTargetType, toolName="Null", baseName="Null") -> vcLinearTarget | vcPtpTarget | vcMultiDriverTarget:
  """Compile robot target from curen robot position, based on motion target type, tool and base data. (Excludes speed data)"""

  # Controller driver positions
  internalDriverPositions = controller.DriverPositions
  externalDriverPositions = controller.ExternalDriverPositions

  # Joint target does not require solving
  if type == vcMotionTargetType.MULTIDRIVER:
    target: vcMultiDriverTarget = controller.createTarget(type)
    target.InternalDriverValues = [(i, val) for i, val in enumerate(internalDriverPositions)]
    target.ExternalDriverValues = externalDriverPositions
    return target

  # Cartesian targets require use of kinematic solver
  if type == vcMotionTargetType.LINEAR or vcMotionTargetType.PTP:
    
    # Kinematics object for controller
    kinematics = controller.Kinematics

    # Create solver for calculating forward kinematic solution for pose
    solver = kinematics.createSolver()
    
    # Define all forvard solution related information
    solver.RootMatrix = controller.Component.WorldPositionMatrix
    solver.BaseMatrix = kinematics.findBase(baseName).PositionMatrix
    solver.ToolMatrix = kinematics.findTool(toolName).PositionMatrix
    
    # Fill in preoperties based on target type
    target: vcLinearTarget | vcPtpTarget = controller.createTarget(type)
    target.Target = solver.forward(internalDriverPositions)
    if isinstance(target, vcLinearTarget):
      target.InternalDriverValues = internalDriverPositions
    if isinstance(target, vcPtpTarget):
      target.DriverReferenceValues = internalDriverPositions
    target.ExternalDriverValues = externalDriverPositions
    
    return target
# Snippet ------------------------------------

def OnReset():
  controller: vcMotionController = getComponent().findBehaviorsByType(vcBehaviorType.ROBOTICS2_MOTIONCONTROLLER)[0]
  target = robotPoseToTarget(controller, vcMotionTargetType.LINEAR, "TOOL[1]", "BASE_2")